﻿using System;
using System.Collections.Generic;
using Allegro.Framework;
using Allegro.MathInterface;
using Allegro.MathLib;

namespace Allegro.Mathlib
{
    public class BrentRootFindingMethod : IRootFinder
    {
        private const int ITMAX = 100;
        private const double EPS = 3.0e-8;

        public double NumIterations;

        public double FindRoot(Func<double, double> f, double x1, double x2, double tol)
        {
            double root = x1;
            int iter;
            double a = x1, b = x2, c = x2;
            double d = 0.0, e=0.0, min1, min2;
            double fa = f(a);
            double fb = f(b);
            double fc, p, q, r, s, tol1, xm;

            if((fa > 0.0 && fb > 0.0)|| (fa < 0.0 && fb < 0.0))
            {
                ErrorHandler.Error("BrentRootFinderMethod.FindRoot: Root must be bracketed");
            }
            fc = fb;
            for(iter = 0; iter < ITMAX; iter++)
            {
                if((fb > 0.0 && fc > 0.0) || (fb < 0.0 && fc < 0.0))
                {
                    // Rename a, b, c and adjust bounding interval d
                    c = a;
                    fc = fa;
                    e = d = b - a;
                }
                if(Math.Abs(fc) < Math.Abs(fb))
                {
                    a = b;
                    b = c;
                    c = a;
                    fa = fb;
                    fb = fc;
                    fc = fa;
                }
                tol1 = 2.0*EPS*Math.Abs(b) + 0.5*tol; // Convergence check
                xm = 0.5*(c - b);
                if (Math.Abs(xm) <= tol1 || fb == 0.0)
                {
                    NumIterations = iter;
                    return b;
                }
                if(Math.Abs(e) >= tol1 && Math.Abs(fa) > Math.Abs(fb))
                {
                    s = fb/fa; //Attempt inverse quadratic interpolation
                    if(a==c)
                    {
                        p = 2.0*xm*s;
                        q = 1.0 - s;
                    }
                    else
                    {
                        q = fa/fc;
                        r = fb/fc;
                        p = s*(2.0*xm*q*(q - r) - (b - a)*(r - 1.0));
                        q = (q - 1.0)*(r - 1.0)*(s - 1.0);
                    }
                    if(p > 0.0)
                        q = -q; // Check whether in bounds
                    p = Math.Abs(p);
                    min1 = 3.0*xm*q - Math.Abs(tol1*q);
                    min2 = Math.Abs(e*q);
                    if(2.0*p < (min1 < min2 ? min1 : min2))
                    {
                        // Accept interpolation
                        e = d;
                        d = p/q;
                    }
                    else
                    {
                        // Interpolation failed, use bisection
                        d = xm;
                        e = d;
                    }
                }
                else
                {
                    // Bounds decreasing too slowly, use bisection
                    d = xm;
                    e = d;
                }
                a = b; // Move last best guess to a
                fa = fb;
                if(Math.Abs(d) > tol1)
                    b += d;
                else
                {
                    b += Math.Abs(tol1)*Math.Sign(xm);
                }
                fb = f(b);
            }
            NumIterations = iter;
            ErrorHandler.Error("BrentRootFinderMethod.FindRoot: Maximum number of iterations exceeded");
            return 0.0;
        }
    }
}
